/**
HardPull Mode
*/
#include "mode.h"
#include "can.h"
#include "stdarg.h"
#include "stdio.h"
#include "spi.h"


extern TIM_HandleTypeDef htim3;
extern uint8_t FLAG_START;
extern uint16_t MOTOR_ID;
extern uint16_t A_Pin[];

void Analog(int Value, ...)
{
  va_list vl;
  va_start(vl, Value);

  if (va_arg(vl, int) == FLAG_LOWSP)
  {
    SetA(DI3, 1);
    SetA(DI4, 1);
    SetA(DI9, 0);
    SetA(DI8, 0);
  }
  else
  {
    SetA(DI3, 0);
    SetA(DI4, 1);
    SetA(DI9, 0);
    SetA(DI8, 0);
  }

  if (L_MOTOR)
    AD5724R_LoadDac(AD5724R_DAC_A, Value);
  if (R_MOTOR)
    AD5724R_LoadDac(AD5724R_DAC_A, -Value);

  va_end(vl);
}


void AnalogKeep(int Value)
{
	  SetA(DI3, 0);
    SetA(DI4, 0);
    SetA(DI9, 0);
    SetA(DI8, 0);
	
	 if (L_MOTOR)
    AD5724R_LoadDac(AD5724R_DAC_A, Value);
  if (R_MOTOR)
    AD5724R_LoadDac(AD5724R_DAC_A, -Value);
}

void SetA(int N, int STATE)
{

  if (N < 1 || N > 8)
  {
    TroubleLED(1);
    return;
  }

  HAL_GPIO_WritePin(A_GPIO, A_Pin[N - 1], (GPIO_PinState)STATE);
}

void SetState()
{

  SetA(DI3, 1);
  SetA(DI4, 0);
  SetA(DI8, 0);
  SetA(DI9, 1); 
}

int* DisPID()
{
			static float p = 1.8, i = 0, d = 1; 
			static int NewArr[2] ={0};
      int err_last = 0, err = 0, err_all = 0;

      err_last = err;
			
		  switch (Sport_Mode)
      {
      case HARDPULL_MODE:
        err = point_low - encode;
        break;
      case DEEPSQUART_MODE:
      {
        if (!FLAG_START)
          err = point_high - encode;
        else
          err = Barbell_MaxLow - encode;

      }
      break;
      case HIGHDROP_MODE:
      {
        err = point_high - encode;
      }
      break;
      case ROW_MODE:
      {
        err = point_low - encode;
      }
      break;
			default:
				err = Barbell_MaxLow - encode;
      }
      err_all += err;

			int analog = p * err_all + i * err + d * (err - err_last);
      if (analog > 2047)
        analog = 2047;
      else if (analog < -2047)
        analog = -2047;
			
			
    
			NewArr[0] = err;
			NewArr[1] = analog;
			return NewArr;
}

void DeepPowerPID(int LRHeight)
{
		static float p_s = 2.5, i_s = 0, d_s = 1; //2.0 0 1.0
		static int err_last_s = 0, err_s = 0, err_previous_s = 0;
		static int power = 0;

		err_last_s = err_previous_s;
		err_previous_s = err_s;
		err_s = height-LRHeight; 

		power = p_s*(err_s-err_previous_s) + i_s*err_s + d_s*(err_s - 2*err_previous_s+err_last_s);

		Power += power;
		
		if(Power - Power_Value > 100)
			Power = Power_Value + 100;
		if(Power - Power_Value < -100)
			Power = Power_Value - 100;

}

void HDropPowerPID(int LRHeight,int addV,int subV)
{
		static float p_s = 2.5, i_s = 0, d_s = 1; //2.0 0 1.0
		static int err_last_s = 0, err_s = 0, err_previous_s = 0;
		static int power = 0;

		err_last_s = err_previous_s;
		err_previous_s = err_s;
		err_s = height-LRHeight; 

		power = p_s*(err_s-err_previous_s) + i_s*err_s + d_s*(err_s - 2*err_previous_s+err_last_s);

		Power -= power;
		
		if(Power - Power_Value > 100)
			Power = Power_Value + addV;
		if(Power - Power_Value < -100)
			Power = Power_Value - subV;

}


void SyncRetStart()
{
    flag = F_NONE;
    get_height = 0;
    FLAG_START = 0;
		Power = Power_Value;  
	
	  CAN_LoadDataFrame(MOTOR_ID, CMD_BACK, 0x0001);
    HAL_CAN_Transmit_IT(&hcan);
}

void Dis_SelfTest(int Dis_I,int Dis_O)
{
  static int cnt;
  if (Dis_I > 800 || Dis_O > 800)
  {
    cnt++;
  }
  else
  {
    cnt = 0;
  }

  if (cnt > 6)
  {
    SetState();  
    cnt = 0;

    if (Sport_Mode == DEEPSQUART_MODE)
    {
      flag = F_NONE;
			get_height = 0;
			FLAG_START = 0;
			Power = Power_Value;
      CAN_LoadDataFrame(MOTOR_ID, CMD_BACK, 0x0001);
      HAL_CAN_Transmit_IT(&hcan);
    }
    error = E_DISLR;

  }
  else
   error = E_NONE;

}

void Speed_SelfTest(int Speed)
{

  if (Speed < -20)
  {
    SetState();
    error = E_SPEED ;
		HAL_Delay(1000);
		if (Sport_Mode == DEEPSQUART_MODE)
    {
      flag = F_NONE;
			get_height = 0;
			FLAG_START = 0;
			Power = Power_Value;
      CAN_LoadDataFrame(MOTOR_ID, CMD_BACK, 0x0001);
      HAL_CAN_Transmit_IT(&hcan);
    }
 
  }
  else
     error = E_NONE;

}

void goPress(int Power, int err)
{

  int Output = Power;
  if (Power < 500)
    Output = Power;
  else if (Power < 1500)
  {
    if (err > -500)
    {
      Output = 500 + (-err - 200) * 2;
      if (Output > Power)
        Output = Power;
    }
    else
      Output = Power;
  }
  else
  {
    if (err > -1000)
    {
      Output = 500 + (-err - 200) * 2;
      if (Output > Power)
        Output = Power;
    }
    else
      Output = Power;
  }

  Analog(-Output);
}





void HardPull(int err,int analog)
{
    if (err > -100)
    {
        Analog(analog, FLAG_LOWSP);
    }
    else
    {
        goPress(Power,err);
    }
}

void DeepSquart(int err,int analog,int speed){
		static int adc_confirm;
	
		if (flag == F_NONE)
		{
			FLAG_START = 0;
			Analog(analog, FLAG_LOWSP);
			if (abs(err) < 200)
			{
				 CAN_LoadDataFrame(MOTOR_ID, CMD_START, 0x0002);
				 HAL_CAN_Transmit_IT(&hcan);
				 flag = F_START;
			}
		}

		if (flag == F_START)
		{
			if (!FLAG_START)
			{
				if (adc > 70)
					adc_confirm++;
				else
					adc_confirm = 0;

				if (L_MOTOR)
				{
					if (adc_confirm > 3)
					{
						adc_confirm = 0;
						CAN_LoadDataFrame(L_MOTOR_ID, CMD_START, 0x0001);
						HAL_CAN_Transmit_IT(&hcan);
						FLAG_START = 1;
					}
					else
					{
						SetState();
					
					}
				}

				if (R_MOTOR)
				{
					if (adc_confirm > 3)
					{
						adc_confirm = 0;
						CAN_LoadDataFrame(R_MOTOR_ID, CMD_START, 0x0001);
						HAL_CAN_Transmit_IT(&hcan);
						FLAG_START = 1;
					}
					else
					{
						SetState();
					
					}
				}
			}
			else
			{
				if(encode < point_high)
					Dis_SelfTest(Dis_I,Dis_O);
				Analog(-Power);
				if (encode < point_low)
				{
					flag = F_END;
					FLAG_2S = 1;
				}
			}
		}

		if (flag == F_END)
		{
			Dis_SelfTest(Dis_I,Dis_O);
			if (encode < point_low)
			{
				if (Count2s > 200)
				{
					FLAG_2S = 0;
					Count2s  =0;
					SyncRetStart();
          error = E_EXHAUSTED;
				}
        else
        {
          error = E_NONE;
        }
				
	  if(encode < Barbell_MaxLow)
				{
					SetState();
				}
			}
			else
			{
				FLAG_2S = 0;
				Speed_SelfTest(speed);
				if (encode > point_high)
					flag = F_START;
			}
		}
}

void HighDrop(int err,int analog, int speed)
{
		static int adc_confirm;
		if (flag == F_NONE)
			{
				Analog(analog, FLAG_LOWSP);
				if (abs(err) < 200)
				{
					flag = F_START;
					CAN_LoadDataFrame(MOTOR_ID, CMD_START, 0x0002);
					HAL_CAN_Transmit_IT(&hcan);
				}
			}
			else if (flag == F_START)
			{

				if (adc < -80)
					adc_confirm++;
				else
					adc_confirm = 0;

				if (adc_confirm > 3)
				{
					CAN_LoadDataFrame(MOTOR_ID, CMD_START, 0x0001);
					HAL_CAN_Transmit_IT(&hcan);
					flag = F_END;
					adc_confirm = 0;
				}
				else
				{
					SetState();
				}

			}
			else if (flag == F_END)
			{
				Analog(Power);
				if(speed > -10)
				{
					 Analog(700);
				}
				if (encode > point_high + 15*CoefficientC2E)
				{
					SetState();
					FLAG_2S = 1;
					if(Count2s > 200)
					{  
						FLAG_2S = 0;
						Count2s = 0;
						SyncRetStart();
						//flag = F_NONE;
					}
					
				}
			}
}


void FlatBenchPress(int err , int analog)
{
			static int adc_confirm;
	     if (flag == F_NONE)
          {

            FLAG_START = 0;
            Analog(analog, FLAG_LOWSP);
            if (abs(err) < 100)
            {
              flag = F_START;
              if (L_MOTOR)
              {
                CAN_LoadDataFrame(L_MOTOR_ID, CMD_START, 0x0002);
                HAL_CAN_Transmit_IT(&hcan);
              }

              if (R_MOTOR)
              {

                CAN_LoadDataFrame(R_MOTOR_ID, CMD_START, 0x0002);
                HAL_CAN_Transmit_IT(&hcan);
              }
            }
          }
          else if (flag == F_START)
          {
            if (adc > 90)
              adc_confirm++;
            else
              adc_confirm = 0;

            if (L_MOTOR)
            {
              if (adc_confirm > 5)
              {
                adc_confirm = 0;
                CAN_LoadDataFrame(L_MOTOR_ID, CMD_START, 0x0001);
                HAL_CAN_Transmit_IT(&hcan);
                flag = F_END;
              }
              else
              {
                SetState();
              }
            }

            if (R_MOTOR)
            {
             if (adc_confirm > 5)
              {
                adc_confirm = 0;
                CAN_LoadDataFrame(L_MOTOR_ID, CMD_START, 0x0001);
                HAL_CAN_Transmit_IT(&hcan);
                flag = F_END;
              }
              else
              {
                SetState();
              }
            }
          }
          else if (flag == F_END)
          {
            Analog(-Power);
            if (encode < point_low)
            {
              SetState();
              FLAG_2S = 1;
              if (Count2s > 200)
              {
                FLAG_2S = 0;
                flag = F_NONE;
              }
            }
          }


}


void RowMode(int err,int analog)
{
  HardPull(err,analog);
}


int Start_SelfTest()
{
	
  Analog(-400, FLAG_LOWSP);
  int encode_t = 0, _encode_t = 0;
  HAL_Delay(200);

  if (L_MOTOR)
    encode_t = -(int16_t)(htim3.Instance->CNT); //
  if (R_MOTOR)
    encode_t = (int16_t)(htim3.Instance->CNT); //
	
  while (abs(_encode_t - encode_t) > 20)
	{
		HAL_CAN_Receive_IT(&hcan, CAN_FIFO0);
    _encode_t = encode_t;
    Analog(-400, FLAG_LOWSP);
    HAL_Delay(100);
		height = CoefficientE2C * encode_t;
    if (L_MOTOR)
		{
		  encode_t = -(int16_t)(htim3.Instance->CNT); //
			CAN_LoadDataFrame(L_MOTOR_ID, CMD_HEIGHT, height);
      HAL_CAN_Transmit_IT(&hcan);
			HeightCheck(height,R_Height);
		}
     
    if (R_MOTOR)
		{
			 encode_t = (int16_t)(htim3.Instance->CNT); //
			 CAN_LoadDataFrame(L_MOTOR_ID, CMD_HEIGHT, height);
       HAL_CAN_Transmit_IT(&hcan);
			 HeightCheck(height,L_Height);

		}


		OLED_Printf(0, 2, "%5d  %5d %5d", height, R_Height , L_Height);		
  }

  return encode_t;
}


void ModeChange()
{			switch (mode)
      {
				 case M_SetMode:
					{
							

						  SetA(DI1,1);
						  //When change mode , the motor maybe stop , so we should restart the motor before selftest
						  if(Sport_Mode != STOP_MODE)
								{
								encode_correction = Start_SelfTest();
								get_height = 0;
								}
								else
								{
								get_height = 1;
								}
						  point_low = (int)(Set_low - MechanicalHeight) * CoefficientC2E;
							point_high = (int)(Set_high - MechanicalHeight) * CoefficientC2E;
							height = CoefficientE2C * encode;

							FLAG_2S = 0;
							Count2s = 0;
							FLAG_EMERGENCY_BRAKING = 0;
							
							FLAG_START = 0;
							flag = F_NONE;
							FLAG_STOP = 1;
							
							
							Power_Value = Power;
							mode = M_NONE;
						
							if (L_MOTOR)
							{
								MOTOR_ID = L_MOTOR_ID;

								switch(Sport_Mode)
								{
									case DEEPSQUART_MODE:
										//Power += 50;
									break;
									case HIGHDROP_MODE:
									{
									 if(Power < 400)
										 Power = 400;
									 Power -= 25;
									}
									break;
								}
								
							}
							if (R_MOTOR)
							{
								MOTOR_ID = R_MOTOR_ID;
								
								switch(Sport_Mode)
								{
									case DEEPSQUART_MODE:
									//	Power -= 25;
									break;
									case HIGHDROP_MODE:
									{
										if(Power < 400)
											Power = 400 ;
										Power += 50;
									}

									break;
								}
							}
						

					}
					break;
					default:
					{
						 mode = M_NONE;
					}
					break;
      }



}
